センサステアリング実験機

センサステアリング方式・・・

センサステアリング方式とエンコーダコロコロの制御の仕組みが今ひとつピンと来ないので，遅くてもいいから実際に作ってみようと思い，夕方から製作開始，ありあわせの部品でここまでできた．制御にはArduinoを使います．

エンコーダは安いやつです．

たぶんあと５時間くらいあればハードはできます．でも次はいつ時間がとれるか分かりません．

－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－
センサステアリング方式のロボトレーサの実験機，ハード完成

昨日から作り始めたセンサステアリング方式のロボトレーサの実験機，途中でほっとくと気になるのでまた夕方から作り始めていまハードが完成しました．製作時間はトータル１０時間くらい．
頭も半田ごても冷やしてから，回路をひと通り確認した後，モータやサーボに負荷をかけないように簡単な初期化プログラムを書き込んでから電源を入れてみます．

上から↓

エンコーダ部分↓

重量は，単３型ニッケル水素電池４本を使うと398g．
900mAh,7.4Vリポを使うと360ｇでした．重たいです．

－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－
センサステアリング方式のロボトレーサの実験機，ハードの制約がありフトが難しい．

センサのアームを最初長めに作ってたのですが，長すぎたようなのでカットして短くしました．
回路をチェックして，１箇所だけハンダミスがあり，修正．
簡単なプログラムを入れて動き出しました．
でも，ハードの制約から制御するのはけっこう難しいです．
まず，Arduinoでサーボとタイマー割り込みとPWMによるアナログ出力は同時に使えなかったのを見落としてて，モータのPWMが使えません．ソフト的にPWMをかけるにしても，処理が遅くてタイマーが5msより短くできないため，PWMというより，ON-OFFのタイミングを適当に変えた感じ．モータには結構な負荷がかかってるはずです．
モータが非力でギヤ比が大きく，レスポンスが遅すぎ．加速も減速も遅く，遅れが出るので制御が難しいです．
サーボのレスポンスも遅いのでラインを追従させるのが難しいです．
安価なエンコーダを使ったのですが，もともと性能が悪いのか，加工時に壊してしまったのか，裏ぶたに振動を加えるとチャタリング？で４回転分くらいカウントがずれます．速度フィードバックに不安ありです．
ジャイロは秋月さんで売ってるムラタのジャイロ．低回転用なので急に回すと飽和して非線形になります．
でも，制御の原理は分かったので安心しました．あとは気長にソフトに改良を加えて，ちゃんと走るようにしたら実験終わり．

なんでもないコーナーでコースアウトしてしまう情けない動画はこちら．

－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－
Arduinoでサーボとタイマー割り込みとPWMを２チャンネル使うには・・・

忘れないうちにメモ．

Arduinoでサーボとタイマー割り込みとPWMを２チャンネル使うには一工夫いります．
内部のタイマーユニットの競合を考えて使い分けないといけません．
詳しくはこちらのサイトの書かれています．

http://blog.livedoor.jp/kan0ken1/archives/51843697.html

Arduinoの標準のPWMによるアナログ出力であるanalogWriteを使おうとすると，MsTimer2は使えないことになり，別のタイマーを使わないといけません．
ArduinoのPlaygroundにTimer1ライブラリが公開されており，タイマー割り込みが使えます．

http://playground.arduino.cc/Code/Timer1

ところが，Timer1はArduinoの標準のサーボライブラリと競合してしまいサーボライブラリが使えませんが，Timer1ではpwm出力も同時に利用することができます．このpwm出力を使ってサーボをコントロールできます．

ということで，
ラインセンサやジャイロの監視　－＞　Timer1によるタイマー割り込み
ステアリングサーボ　－＞　Timer1　の　pwm出力（9ピン）
左右モータ　－＞　analogWrite　（Timer2を使ったpwm（3,11ピン），回転方向は12,13ピン）
エンコーダA相の監視　－＞　チャンネル0（2ピン）の外部割り込み，B相は４ピン

と割り当てることで解決しました．
光センサ５本（A0-A4），ジャイロ１本（A5）でアナログポート全部，
光センサ用の赤外LED点灯（10ピン），
スタートスイッチ（5ピン）
セットスイッチ（6ピン）
表示用LED０（7ピン）
表示用LED１（8ピン）
ブザー（LED1と兼用8ピン）

で開いてるピンなし．ArduinoUNOをほぼフル機能で使ってる感じです．

－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－
センサステアリング方式のロボトレースの実験機，終了

左右モータの平均速度とエンコーダによる速度とでフィードバックをかけて平均速度を制御をして，
ステアリングをPID制御，さらにセンサがラインから外れた時に，履歴で外れた方向を持っておいて例外処理でセンサを強引に戻して外れにくくして，R10のターンもできるようになりました．
ジャイロによる補正も加えてみて走行の安定化も確認できました．
基本的な制御ループはできたので，地道にパラメータ調整すればもうちょっと速くなります．
マーカー処理も付け加えました．ただし，スタート，ゴールは直線上なので判断できますが，
コーナーマーカーをきっちり読むのは難しいです．ちょっと蛇行すると読み飛ばします．
機体の寸法設計やステアリングと機体の追従性をちゃんと考えて作らないといけないようです．
左右モータの速度を独立に計測できないので，左右モータの特性のバラつきに大きく依存します．使用したモータ・ギヤボックスがジャンクから拾ってきたこともあって，倍くらい速度が違い，制御が難しいです．そのせいで速度をあげようとすると連動してあちこちパラメータを加減してやらないといけません．
あくまで実験機で限界も見えてるので，これで終了とします．

ここまでの動画→ センサステアリング方式のロボトレーサの実験機，その２

－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－
ちなみにソースはこんな感じ．参考になる部分があったらいいですが，なければ無視してください．

//--------------------------------------------------------------------------
// センサステアリング方式のロボトレーサ実験機
// 2013.12.2-9 葉山清輝（熊本高専）
//--------------------------------------------------------------------------

#include < Servo.h >
#include < TimerOne.h >

// Arduinoのピン割り当て　---------------------------------------------------
// エンコーダ入力
#define PA 0 // エンコーダA相割込み（digital2 - > interrpt 0 ）
#define PB 4 // エンコーダB相デジタル入力(HIGHかLOWかによって正転・逆転の判断)
// 入力スイッチ
#define SET 6 // セットスイッチ，デジタル入力
#define START 5 // スタートスイッチ，デジタル入力
// サーボ出力ステアリング
#define SRV_STR 9
// 左右モータ出力用設定
#define PWM_R 3 // 右モータのPWM出力ピン
#define PWM_L 11 // 左モータのPWM出力ピン
#define DIR_R 12 // 右モータの方向指示（HIGH:正転，LOW：逆転）
#define DIR_L 13 // 左モータの方向指示（HIGH:正転，LOW：逆転）
// LED出力
#define LED0 8 // 表示用LED,後ろ，ポートが足りなかったのでブザーも並列に接続してある．
#define LED1 7 // 表示用LED，前
// 赤外線出力
#define IR_OUT 10 // ライン及びマーカーセンサ用の赤外線出力
// センサ入力
#define SENS_FR A0 // ラインセンサ右
#define SENS_FM A1 // ラインセンサ中央
#define SENS_FL A2 // ラインセンサ左
#define SENS_SG A3 // スタート・ゴールマーカーセンサ
#define SENS_C A4 // コーナーマーカーセンサ
// ジャイロ入力
#define GYRO_IN A5

// 各種定義値-----------------------------------------------------------------
#define STH 200 // センサをディジタル処理するときのしきい値
#define SGTH 10 // threshold value for start/goal marker
#define CTH 10
#define KVP 10 // 速度のフィードバックの比例値
#define KVI 0.3 // 速度のフィードバックの積分値
#define KGYRO 1 // 速度のフィードバックのジャイロによる安定化
#define KLD 3 // ラインセンサフィードバックの微分値
#define GYMAX 35 // ジャイロ出力の最大値，ノイズの影響を除くためこれ以上の値は無視
#define STRN 285 // ステアリングのニュートラルポジション
#define STRMIN 120 // ステアリングの最小値
#define STRMAX 520 // ステアリングの最大値
#define OFST 3 // ステアリングの最大値

// 変数定義-----------------------------------------------------------------
volatile long cnt=0; // 回転数カウント
volatile long cntB=0; // 回転数カウント
volatile int spd=0; // エンコーダ差分による速度値

volatile int goff=0; // ジャイロオフセット
volatile int gout=0; // ジャイロ出力
volatile int angle=0; // ジャイロによる角度検出値

volatile int sensFR, sensFRB; // ラインセンサ右，赤外線点灯前（以下同様）
volatile int sensFM, sensFMB; // ラインセンサ中央
volatile int sensFL, sensFLB; // ラインセンサ左
volatile int sensSG, sensSGB; // スタート・ゴールマーカー
volatile int sensC, sensCB; // コーナーマーカー

volatile int markerSG, markerC, markerX; // マーカー検出処理用変数
unsigned char frun, fmarker; // スタート，ゴールの状態遷移

volatile int posStr; // ステアリングのポジション
volatile int posLine, posLineB; // ステアリングのポジション
volatile int lineOut; //　ラインアウトしたときの履歴処理用変数

volatile int vR,vL,vF,vI; // 速度および速度制御係数
volatile int pwmR,pwmL; // 左右モータPWM出力値

volatile int cntStr=0; // ステアリング制御頻度のカウンタ
volatile int mf=0; // モーターフラグ，モータ回すか回さないか
volatile int cntSp=0; // スピード制御頻度のカウンタ
volatile int cntLineOut; // ラインアウトのカウント，この値以下なら直前のラインアウト方向を元に線を探し続ける

volatile int bp=0; // ブザー用のカウンタ

int pmode=0; // プログラムモード(メインメニューで分岐)

//--------------------------------------------------------------------------
// 割り込みによるエンコーダ監視
void cntupdwn(){
if (digitalRead(PB)==HIGH) cnt++; else cnt--;
//delayMicroseconds(100); // ディレイでチャタリング防止を考えたがだめっぽいのでコメント化
}

//--------------------------------------------------------------------------
// タイマー割込み処理
void timerIsr() {
// ジャイロ監視
gout=(analogRead(GYRO_IN)-goff);
if (abs(gout)< GYMAX) angle+=gout;

// センサ監視
sensFRB=analogRead(SENS_FR); // 赤外線LED消灯時のセンサの読み，以下同様
sensFMB=analogRead(SENS_FM);
sensFLB=analogRead(SENS_FL);
sensSGB=analogRead(SENS_SG);
sensCB= analogRead(SENS_C);
digitalWrite(IR_OUT, HIGH); // 赤外線LED点灯
delayMicroseconds(20); // ちょっと待つ
sensFR=analogRead(SENS_FR)-sensFRB; // 赤外線点灯前後の差でラインの読みとする，以下同様
sensFM=analogRead(SENS_FM)-sensFMB;
sensFL=analogRead(SENS_FL)-sensFLB;
sensSG=analogRead(SENS_SG)-sensSGB;
sensC=analogRead(SENS_C)-sensCB;
digitalWrite(IR_OUT, LOW); // 赤外線LED消灯

// マーカー処理
if (sensSG > STH) markerSG++; else if (markerSG > 0) markerSG--; // マーカーを積算で読む
if (sensC > STH ) markerC++; else if (markerC > 0) markerC--;
// クロスラインの検出
if (markerSG > 2 && markerC > 2) markerX=1; // 両方のマーカーが同時に検出された時クロスラインと判断
if (markerX==1 && markerSG==0 && markerC==0) markerX=0; // クロスラインの時はマーカーと処理させない

// ラインセンサ読み取り
posLineB=posLine; // ラインセンサの変化を得るため前の値を記録
if (sensFR > sensFL) posLine=sensFR-sensFM+700; else posLine=sensFM-sensFL-720; // ラインセンサ読み取り，およそプラスマイナス1200の値になる
posLine=(float)posLine/120; //　値が大きすぎるのて適当に割る
if (sensFM < STH && sensFR > STH) lineOut=10; // どちら側にあるか履歴として残しておく
if (sensFM < STH && sensFL > STH) lineOut=-10;
if (sensFR < STH && sensFM < STH && sensFL < STH) posLine=lineOut; // ラインを見失った時，どちらに外れたか履歴を使う

// ステアリング制御
if (cntStr > 0) cntStr--; else cntStr=3; // 制御頻度を４回に１回（20ms毎）にする
if (cntStr==0){
if (sensFR < STH && sensFM < STH && sensFL < STH) cntLineOut++; else cntLineOut=0; // ラインアウトしてもしばらく処理させるためのカウンタ

if (cntLineOut < 15){
posStr-=posLine; // ステアリングを積算処理
posStr-=(float)(posLine-posLineB)*KLD; // ステアリングを微分（差分）処理
posStr=constrain(posStr,STRMIN,STRMAX); // ステアリングの制御幅を制限
Timer1.pwm(SRV_STR, posStr); // 周期5000(5ms)のとき120-520 の範囲でサーボの右いっぱいから左いっぱい，中央が約320
}
}

// 速度制御
cntSp++; if (cntSp > 2){ //　速度制御の頻度を適当にきめる
cntSp=0;
// 速度計算
spd=cnt-cntB; cntB=cnt; // エンコーダの差から速度を得る
vF=(vR+vL)/2; // 左右設定速度の平均値を得る

if(abs(vF-spd) < 10) vI+=(float)(vF-spd)*KVI; // エンコーダのノイズのせいで大きな値が入るので上限をきめる

pwmR=vR*KVP+vR*vI+gout*KGYRO+OFST; // 右モータのPWM値を比例，積分，ジャイロで決める
pwmL=vL*KVP+vL*vI-gout*KGYRO-OFST; // 左，同上

pwmR=constrain(pwmR, -255,255); // PWM値の上限，下限を制限
pwmL=constrain(pwmL, -255,255);
if(pwmR > 0) digitalWrite(DIR_R, HIGH); else digitalWrite(DIR_R, LOW); // モータの方向設定
if(pwmL > 0) digitalWrite(DIR_L, HIGH); else digitalWrite(DIR_L, LOW);
if (mf==1) {
analogWrite(PWM_R, abs(pwmR)); // モータに出力
analogWrite(PWM_L, abs(pwmL));
} else {
analogWrite(PWM_R,0); // モータフラグが０の時は，モータに出力しない
analogWrite(PWM_L,0);
}
//Serial.print(pwmF); Serial.print(" "); Serial.println(spd); // 制御値モニタ用，コメントアウト
}

// ブザーを鳴らす
if (bp > 0){
bp--;
digitalWrite(LED0, digitalRead(LED0) ^ 1 ); // 交互にON-OFFする
}
}

//--------------------------------------------------------------------------
// ブザー
//--------------------------------------------------------------------------
void beep(int n){
bp=n; // ブザー用カウンタのセット
}

//--------------------------------------------------------------------------
// LED表示　（ LED1,LED0 に２進表示）
void dispLED(byte n){
switch(n){
case 0: digitalWrite(LED1, LOW); digitalWrite(LED0, LOW); break;
case 1: digitalWrite(LED1, LOW); digitalWrite(LED0, HIGH); break;
case 2: digitalWrite(LED1, HIGH); digitalWrite(LED0, LOW); break;
case 3: digitalWrite(LED1, HIGH); digitalWrite(LED0, HIGH); break;
}
}

//--------------------------------------------------------------------------
// パラメ－タ初期化
void initPara(){
posStr=STRN;
vR=vL=0;
pwmR=pwmL=0;
spd=0; mf=0;
}

//--------------------------------------------------------------------------
// 走行
//--------------------------------------------------------------------------
void Run(){
int cntGoal=0; // couter for run after goal
mf=1;
vI=0;
cnt=0;
cntLineOut=0;
markerSG=0; markerC=0; markerX=0; fmarker=0;
frun=0; dispLED(0);

while(digitalRead(START)==HIGH){
vR=3;
vL=5;
vR+=(posStr-STRN)/40;
vL-=(posStr-STRN)/40;
vR=constrain(vR,-20,20);
vL=constrain(vL,-20,20);

//if(spd > 5) digitalWrite(LED0, HIGH); else digitalWrite(LED0,LOW);
if (cntLineOut > 15) break;

// corner marker check
if (markerX==1) fmarker=0;
if (markerX==0 && fmarker==0 && markerC > CTH) fmarker=1;
if (markerX==0 && fmarker==1 && markerC==0){
fmarker=0; beep(20);
}
// start/goal marker check
if (frun==0 && markerSG > SGTH) frun=1; // start marker detect
if (frun==1 && markerSG==0){ // start marker fix
frun=2; beep(20); dispLED(1);
}
if (frun==2 && markerSG > SGTH) frun=3; // goal marker detect
if (frun==3 && markerX==1) frun=2; // ignor cross line
if (frun==3 && markerSG==0){ // goal marker fix
frun=4; beep(20); dispLED(2);
cntGoal=cnt;
}
if (frun==4 && cnt > (cntGoal+300)) break; // 約２０cm先まで走ってストップ

delay(20);
}
}

//--------------------------------------------------------------------------
// セットアップ（最初に一度だけ実行）
//--------------------------------------------------------------------------
void setup()
{
int i;

pinMode(SRV_STR, OUTPUT);

attachInterrupt(PA, cntupdwn, RISING); // pin2, エンコーダA
pinMode(PB, INPUT); // pin4, エンコーダB相

pinMode(SET, INPUT); // セットスイッチ
pinMode(START, INPUT); // スタートスイッチ

pinMode(IR_OUT,OUTPUT); // センサ赤外線LED点灯用
digitalWrite(IR_OUT, LOW);

pinMode(LED0, OUTPUT); // 状態モニタ用LED
pinMode(LED1, OUTPUT); // 状態モニタ用LED

pinMode(PWM_R, OUTPUT);
pinMode(PWM_L, OUTPUT);
pinMode(DIR_R, OUTPUT);
pinMode(DIR_L, OUTPUT);
analogWrite(PWM_R, 0);
analogWrite(PWM_L, 0);
digitalWrite(DIR_R, HIGH);
digitalWrite(DIR_L, HIGH);

// ジャイロオフセット取得
delay(1000);
for(i=0;i < 20;i++){ goff += analogRead(GYRO_IN); delay(10); }
goff=goff/20+1;

// ターマーセット
Timer1.initialize(5000); // 5ms毎にタイマー割り込み
Timer1.attachInterrupt( timerIsr );
Timer1.pwm(SRV_STR,STRN);

// シリアル通信スタート
Serial.begin(9600);

}

//--------------------------------------------------------------------------
// ループ（繰り返し実行）
//--------------------------------------------------------------------------
void loop(){
initPara();
dispLED(pmode); // プログラムモードをLEDに2進表示

while (digitalRead(START)==HIGH){ // スタートスイッチが押されたら指定プログラムを実行
if (digitalRead(SET)==LOW) { // セットスイッチが押されたらプログラムモードを進める
delay(10); // チャタリング防止
while(digitalRead(SET)==LOW);
delay(10); // チャタリング防止
pmode++; if (pmode > 3) pmode=0; // プログラムモードを一つ増やす
dispLED(pmode); // プログラムモードをLEDに2進表示
beep(20);
}
}
// スタートスイッチが押されたら処理プログラムへ分岐
dispLED(0);
delay(500); // 0.5秒待つ

switch(pmode){ // 選択されたプログラムモードのプログラムを実行
case 0: Run(); break;
case 1: testSensors2(); break;
case 2: testEncoder(); break;
case 3: testLine(); break;
}
delay(500); // 0.5秒待つ
}

//--------------------------------------------------------------------------
// 以下テストプログラム
//--------------------------------------------------------------------------
void testLine(){
while(1){
Serial.print(sensFR); Serial.print(" "); Serial.print(sensFM); Serial.print(" "); Serial.print(sensFL); Serial.print(" "); Serial.println(posLine);
delay(50);
}
}

//--------------------------------------------------------------------------
void testMotor(){
while(1){
if (digitalRead(START)==HIGH) vR=0; else vR=1;
if (digitalRead(SET) ==HIGH) vR=0; else vR=2;
}
}

//--------------------------------------------------------------------------
void testServo(){
while(1){
if (digitalRead(START)==LOW) posStr++;
if (digitalRead(SET)==LOW) posStr--;
// steer.write(posStr);
Serial.println(posStr);
delay(50);
}
}

//--------------------------------------------------------------------------
void testServoFollow(){
while(digitalRead(START)==HIGH){

if (sensFR < STH && sensFM < STH && sensFL < STH) ; else {
posStr-=posLine;
// steer.write(posStr);
}
Serial.println(posStr);
delay(20);
}
}

//--------------------------------------------------------------------------
void testEncoder(){
while(digitalRead(START)==HIGH){
if (digitalRead(SET)==LOW) cnt=0;
Serial.print(cnt); Serial.print(" "); Serial.println(spd);
delay(50);
}
}

//--------------------------------------------------------------------------
void testSensors(){
while(digitalRead(START)==HIGH){
Serial.print(sensFR); Serial.print(" ");
Serial.print(sensFM); Serial.print(" ");
Serial.print(sensFL); Serial.print(" ");
Serial.print(sensSG); Serial.print(" ");
Serial.print(sensC); Serial.print(" ");
Serial.print(gout); Serial.print(" ");
Serial.print(angle); Serial.print(" ");
Serial.print(cnt); Serial.print(" ");
Serial.println();
delay(50);
}
}

void testSensors2(){
while(digitalRead(START)==HIGH){
Serial.print(markerSG); Serial.print(" ");
Serial.print(markerC); Serial.print(" ");
Serial.print(markerX); Serial.print(" ");
Serial.println();
delay(50);
}
}

－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－－